#include "SurfIcpMatcherII.h"
#include <vector>

using namespace std;

SurfIcpMatcherII::SurfIcpMatcherII()
{
	name = "SurfIcpMatcherII";
	printf("new SurfIcpMatcherII\n");
	nr_iter = 10;
	distance_scale = 0.05f;
	distance_threshold = 0.05f * distance_scale;
	surf_threshold = 0.2f;
}

SurfIcpMatcherII::~SurfIcpMatcherII()
{
	printf("delete SurfIcpMatcherII\n");
}

void SurfIcpMatcherII::update(){
}

const bool debugg_SurfIcpMatcherII = false;
Transformation * SurfIcpMatcherII::getTransformation(FrameNode * src, FrameNode * dst)
{
	//pcl::visualization::CloudViewer viewer("Cloud Viewer");
	if(debugg_SurfIcpMatcherII){printf("SurfIcpMatcherI::getTransformation(%i,%i)\n",src->frame->id_number,dst->frame->id_number);}

	IplImage* img_combine;
	int sx;
	int sy;
	if(debugg_SurfIcpMatcherII)
	{
		IplImage* img_a = cvLoadImage( src->frame->rgb_path.c_str(), 1);
		IplImage* img_b = cvLoadImage( dst->frame->rgb_path.c_str(), 1);
	
		sx = img_a->width;
		sy = img_a->height;
		
		img_combine = cvCreateImage(cvSize(2*sx,sy), IPL_DEPTH_8U, 3);
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 0] = img_a->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 1] = img_a->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 2] = img_a->imageData[3 * (j * sx + i) + 2];
			}
		}
	
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 0] = img_b->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 1] = img_b->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 2] = img_b->imageData[3 * (j * sx + i) + 2];
			}
		}
		cvReleaseImage( &img_a );
    	cvReleaseImage( &img_b );
    	cvNamedWindow("Combination", CV_WINDOW_AUTOSIZE );
		cvShowImage("Combination", img_combine);
    	//cvWaitKey(0);
	}
	
	double t1 = get_time_now();
	
	KeyPointSet * src_keypointset 	= src->frame->keypoints;
	KeyPointSet * dst_keypointset 	= dst->frame->keypoints;
	
	KeyPointSet ** src_clusters 	= src->frame->cluster_keypoints;
	KeyPointSet ** dst_clusters 	= dst->frame->cluster_keypoints;
	
	KeyPoint ** src_keypoints	 	= src->frame->keypoints->valid_key_points;
	KeyPoint ** dst_keypoints 		= dst->frame->keypoints->valid_key_points;
	
	int src_nr_points = src_keypointset->nr_valid_points;
	int dst_nr_points = dst_keypointset->nr_valid_points;
	
	float ** 		surf_distances_src		= new float    *[src_nr_points];
	float ** 		match_distances_src		= new float    *[src_nr_points];
	int * 			nr_compare_points_src 	= new int		[src_nr_points];
	KeyPoint *** 	compare_points_src 		= new KeyPoint**[src_nr_points];
	
	float ** 		surf_distances_dst		= new float    *[dst_nr_points];
	float ** 		match_distances_dst		= new float    *[dst_nr_points];
	int * 			nr_compare_points_dst 	= new int		[dst_nr_points];
	KeyPoint *** 	compare_points_dst 		= new KeyPoint**[dst_nr_points];
	
	KeyPoint ** 	src_matches 			= new KeyPoint*[src_nr_points];
	KeyPoint ** 	dst_matches 			= new KeyPoint*[dst_nr_points];
	double t2 = get_time_now();
	
	float * pos_src_x 				= new float[src_nr_points];
	float * pos_src_y 				= new float[src_nr_points];
	float * pos_src_z 				= new float[src_nr_points];
	float * pos_src_x_transform 	= new float[src_nr_points];
	float * pos_src_y_transform 	= new float[src_nr_points];
	float * pos_src_z_transform 	= new float[src_nr_points];
	
	float * pos_dst_x 				= new float[dst_nr_points];
	float * pos_dst_y 				= new float[dst_nr_points];
	float * pos_dst_z 				= new float[dst_nr_points];
	float * pos_dst_x_transform 	= new float[dst_nr_points];
	float * pos_dst_y_transform 	= new float[dst_nr_points];
	float * pos_dst_z_transform 	= new float[dst_nr_points];
	
	for(int i = 0; i < src_nr_points;i++)
	{
		pos_src_x[i] = src_keypoints[i]->position(0);
		pos_src_y[i] = src_keypoints[i]->position(1);
		pos_src_z[i] = src_keypoints[i]->position(2);
		if(debugg_SurfIcpMatcherII){cvCircle(img_combine,cvPoint(src_keypoints[i]->frame_x + sx, src_keypoints[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	for(int i = 0; i < dst_nr_points;i++)
	{
		pos_dst_x[i] = dst_keypoints[i]->position(0);
		pos_dst_y[i] = dst_keypoints[i]->position(1);
		pos_dst_z[i] = dst_keypoints[i]->position(2);
		if(debugg_SurfIcpMatcherII){cvCircle(img_combine,cvPoint(dst_keypoints[i]->frame_x, dst_keypoints[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	if(debugg_SurfIcpMatcherII)
	{
		cvNamedWindow("Combination", CV_WINDOW_AUTOSIZE );
		cvShowImage("Combination", img_combine);
		cvWaitKey(30);
	}
	
	int nr_clusters_search = 100;
	IplImage * img_combine_clone;
	if(debugg_SurfIcpMatcherII)
	{ 
		img_combine_clone = cvCreateImage( cvSize( img_combine->width, img_combine->height ), img_combine->depth, img_combine->nChannels );
	}
	
	for(int i = 0; i < src_nr_points;i++)
	{
		KeyPoint * kp = src_keypoints[i];
		if(debugg_SurfIcpMatcherII){
			cvCopy( img_combine, img_combine_clone, NULL );
			cvCircle(img_combine_clone,cvPoint(kp->frame_x+sx, kp->frame_y), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
		}
		nr_compare_points_src[i]	= 0;
		for(int j = 0; j < nr_clusters_search; j++){nr_compare_points_src[i]+=dst_clusters[kp->word_distances_sort_index[j]]->nr_valid_points;}
		compare_points_src[i]		= new KeyPoint*[nr_compare_points_src[i]];
		surf_distances_src[i]		= new float[nr_compare_points_src[i]];
		match_distances_src[i]		= new float[nr_compare_points_src[i]];
		int added = 0;
		for(int j = 0; j < nr_clusters_search;j++)
		{
			int cluster_number 		= kp->word_distances_sort_index[j];//nr_valid_points
			KeyPointSet * cluster 	= dst_clusters[cluster_number];
			for(int k = 0; k < cluster->nr_valid_points; k++)
			{
				compare_points_src[i][added] = cluster->valid_key_points[k];
				surf_distances_src[i][added] = kp->surf_descriptor->distance(compare_points_src[i][added]->surf_descriptor);
				if(debugg_SurfIcpMatcherII){
					cvLine(img_combine_clone,cvPoint(compare_points_src[i][added]->frame_x, compare_points_src[i][added]->frame_y),cvPoint(kp->frame_x+sx, kp->frame_y),cvScalar(0, 255, 0, 0),1, 8, 0);
					cvCircle(img_combine_clone,cvPoint(compare_points_src[i][added]->frame_x, compare_points_src[i][added]->frame_y), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				}
				added++;
			}
		}
		if(debugg_SurfIcpMatcherII){
			cvNamedWindow("Combination", CV_WINDOW_AUTOSIZE );
			cvShowImage("Combination", img_combine_clone);
			cvWaitKey(30);
		}
	}
		
	for(int i = 0; i < dst_nr_points;i++)
	{
		printf("dst:%i\n",i);
		KeyPoint * kp = dst_keypoints[i];
		if(debugg_SurfIcpMatcherII){
			cvCopy( img_combine, img_combine_clone, NULL );
			cvCircle(img_combine_clone,cvPoint(kp->frame_x, kp->frame_y), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
		}
		nr_compare_points_dst[i] = 0;
		for(int j = 0; j < nr_clusters_search; j++){nr_compare_points_dst[i]+=src_clusters[kp->word_distances_sort_index[j]]->nr_valid_points;}
		
		compare_points_dst[i] 		= new KeyPoint*[nr_compare_points_dst[i]];
		
		surf_distances_dst[i]		= new float[nr_compare_points_dst[i]];
		match_distances_dst[i]		= new float[nr_compare_points_dst[i]];
		int added = 0;
		
		for(int j = 0; j < nr_clusters_search;j++)
		{
			int cluster_number = kp->word_distances_sort_index[j];//nr_valid_points
			KeyPointSet * cluster = src_clusters[cluster_number];
			
			for(int k = 0; k < cluster->nr_valid_points; k++)
			{
				
				compare_points_dst[i][added] = cluster->valid_key_points[k];
				surf_distances_dst[i][added] = kp->surf_descriptor->distance(compare_points_dst[i][added]->surf_descriptor);
				if(debugg_SurfIcpMatcherII){
					cvLine(img_combine_clone,cvPoint(compare_points_dst[i][added]->frame_x+sx, compare_points_dst[i][added]->frame_y),cvPoint(kp->frame_x, kp->frame_y),cvScalar(0, 255, 0, 0),1, 8, 0);
					cvCircle(img_combine_clone,cvPoint(compare_points_dst[i][added]->frame_x+sx, compare_points_dst[i][added]->frame_y), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
				}
				added++;
				
			}
			
		}
		if(debugg_SurfIcpMatcherII){
			cvNamedWindow("Combination", CV_WINDOW_AUTOSIZE );
			cvShowImage("Combination", img_combine_clone);
			cvWaitKey(30);
		}
	}
	Eigen::Matrix4f transformationMat = Eigen::Matrix4f::Identity();
	Transformation * transformation = new Transformation();
	transformation->transformationMatrix = transformationMat;
	transformation->src = src;
	transformation->dst = dst;
	transformation->weight = 100;
	transformation->level = 1;
	transformation->matches_src = new KeyPoint*[src_nr_points];
	transformation->matches_dst = new KeyPoint*[src_nr_points];
	transformation->nr_matches  = 0;
	transformation->next = 0;
	transformation->prev = 0;
	Transformation * first_trans = transformation; 

	float step_size = 1.0f/(float)nr_iter;
	for(float alpha = 0; alpha <= 1.0; alpha+=step_size)
	{

		printf("alpha: %f\n",alpha);

		float mat00 = transformationMat(0,0);
		float mat01 = transformationMat(0,1);
		float mat02 = transformationMat(0,2);
		float mat03 = transformationMat(0,3);
		float mat10 = transformationMat(1,0);
		float mat11 = transformationMat(1,1);
		float mat12 = transformationMat(1,2);
		float mat13 = transformationMat(1,3);
		float mat20 = transformationMat(2,0);
		float mat21 = transformationMat(2,1);
		float mat22 = transformationMat(2,2);
		float mat23 = transformationMat(2,3);
		float mat30 = transformationMat(3,0);
		float mat31 = transformationMat(3,1);
		float mat32 = transformationMat(3,2);
		float mat33 = transformationMat(3,3);

		for(int i = 0; i < src_nr_points;i++)
		{
			float x = pos_src_x[i];
			float y = pos_src_y[i];
			float z = pos_src_z[i];
				
			src_keypoints[i]->tmp_x = x*mat00+y*mat01+z*mat02+mat03;
			src_keypoints[i]->tmp_y = x*mat10+y*mat11+z*mat12+mat13;
			src_keypoints[i]->tmp_z = x*mat20+y*mat21+z*mat22+mat23;
		}
		for(int i = 0; i < dst_nr_points;i++)
		{
			dst_keypoints[i]->tmp_x = pos_dst_x[i];
			dst_keypoints[i]->tmp_y = pos_dst_y[i];
			dst_keypoints[i]->tmp_z = pos_dst_z[i];
		}
		
		for(int i = 0; i < src_nr_points;i++)
		{
			float x = src_keypoints[i]->tmp_x;
			float y = src_keypoints[i]->tmp_y;
			float z = src_keypoints[i]->tmp_z;
			float dx,dy,dz;
			KeyPoint * kp_dst;
			for(int j = 0; j < nr_compare_points_src[i];j++)
			{
				kp_dst = compare_points_src[i][j];
				dx = x-kp_dst->tmp_x;
				dy = y-kp_dst->tmp_y;
				dz = z-kp_dst->tmp_z;
				match_distances_src[i][j] = (1-alpha)*surf_distances_src[i][j] + distance_scale*alpha*(dx*dx + dy*dy + dz*dz);
			}
		}
	
		for(int i = 0; i < src_nr_points;i++)
		{
			src_matches[i] = 0;
			float best_value = 9999999;
			for(int j = 0; j < nr_compare_points_src[i];j++)
			{
				if(match_distances_src[i][j]<best_value){
					best_value = match_distances_src[i][j];
					src_matches[i] = compare_points_src[i][j];
				}
			}
			
			if(debugg_SurfIcpMatcherII){
				cvCopy(img_combine, img_combine_clone, NULL);
				cvLine(img_combine_clone,cvPoint(src_keypoints[i]->frame_x+sx,src_keypoints[i]->frame_y),cvPoint(src_matches[i]->frame_x,src_matches[i]->frame_y ),cvScalar(0, 255, 0, 0),1, 8, 0);
				cvNamedWindow("Combination", CV_WINDOW_AUTOSIZE );
				cvShowImage("Combination", img_combine_clone);
				cvWaitKey(30);
			}
		}
		printf("done:1\n");
		for(int i = 0; i < dst_nr_points;i++)
		{
			dst_matches[i] = 0;
			float best_value = 9999999;
			for(int j = 0; j < nr_compare_points_dst[i];j++)
			{
				if(match_distances_dst[i][j]<best_value){
					best_value = match_distances_dst[i][j];
					dst_matches[i] = compare_points_dst[i][j];
				}
			}
			
			if(debugg_SurfIcpMatcherII){
				cvCopy(img_combine, img_combine_clone, NULL);
				cvLine(img_combine_clone,cvPoint(dst_keypoints[i]->frame_x,dst_keypoints[i]->frame_y),cvPoint(dst_matches[i]->frame_x + sx,dst_matches[i]->frame_y ),cvScalar(0, 255, 0, 0),1, 8, 0);
				cvNamedWindow("Combination", CV_WINDOW_AUTOSIZE );
				cvShowImage("Combination", img_combine_clone);
				cvWaitKey(30);
			}
		}
		printf("done:2\n");
		for(int i = 0; i < src_nr_points;i++)
		{
			exit(0);
		}
		/*
		Transformation * transformation2;
		if(debugg_SurfIcpMatcherI){
			transformation2 = new Transformation();
			transformation2->src = src;
			transformation2->dst = dst;
			transformation2->weight = alpha;
			transformation2->level = 1;
			transformation2->matches_src = new KeyPoint*[src_nr_points];
			transformation2->matches_dst = new KeyPoint*[src_nr_points];
			transformation2->nr_matches  = 0;
		}
		
		pcl::TransformationFromCorrespondences tfc;
		int nr_matches = 0;
		float threshold = distance_threshold*alpha + (1 - alpha)*surf_threshold;
		//printf("-------------------------------------------------------\n");
		for(int i = 0; i < src_nr_points;i++)
		{
			if(i == dst_matches[src_matches[i]] && match_distances[i][src_matches[i]] < threshold){
				src_match_id[nr_matches] = src->frame->keypoints->valid_key_points[i];
				dst_match_id[nr_matches] = dst->frame->keypoints->valid_key_points[src_matches[i]];
				tfc.add(src_match_id[nr_matches]->position, dst_match_id[nr_matches]->position);
				if(debugg_SurfIcpMatcherI){
					transformation2->matches_src[nr_matches] = src_match_id[nr_matches];
					transformation2->matches_dst[nr_matches] = dst_match_id[nr_matches];
					transformation2->nr_matches = nr_matches+1;
				}else{
					transformation->matches_src[nr_matches] = src_match_id[nr_matches];
					transformation->matches_dst[nr_matches] = dst_match_id[nr_matches];
					transformation->nr_matches = nr_matches+1;
				}
				nr_matches++;
			}
		}		
		if(debugg_SurfIcpMatcherII){
			transformation2->transformationMatrix = tfc.getTransformation().matrix();
			transformation->next = transformation2;
			transformation2->prev = transformation;
			transformation = transformation2;
			transformationMat = transformation2->transformationMatrix;
		}else
		{
			transformationMat = tfc.getTransformation().matrix();
			transformation->transformationMatrix = transformationMat;
		}
		*/
		/*
		if(debugg_SurfIcpMatcherII)
		{
			cvNamedWindow("Combination", CV_WINDOW_AUTOSIZE );
			cvShowImage("Combination", img_combine);
    		cvWaitKey(0);
    		cvReleaseImage( &img_combine );
    	}
    */
	}
	
	printf("Done...\n");
	exit(0);
	
	return first_trans;
}
